﻿using System;
using System.Diagnostics;
using System.Globalization;
using TomanuExtensions;
using YAXLib;

namespace RaytracerLib.MathLib
{
    /// <summary>
    /// For 2D.
    /// </summary>
    [DebuggerStepThrough]
    public struct Matrix3
    {
        [YAXAttributeForClass]
        public readonly double M11, M12, M13;

        [YAXAttributeForClass]
        public readonly double M21, M22, M23;

        [YAXAttributeForClass]
        public readonly double M31, M32, M33;

        public static readonly Matrix3 Identity = new Matrix3(

           1, 0, 0,
           0, 1, 0,
           0, 0, 1);

        public static readonly Matrix3 Zero = new Matrix3(

           0, 0, 0,
           0, 0, 0,
           0, 0, 0);

        public Matrix3(double[] a_ar)
        {
            Debug.Assert(a_ar.Length == 9);

            M11 = a_ar[0];
            M12 = a_ar[1];
            M13 = a_ar[2];

            M21 = a_ar[3];
            M22 = a_ar[4];
            M23 = a_ar[5];

            M31 = a_ar[6];
            M32 = a_ar[7];
            M33 = a_ar[8];
        }

        public Matrix3(double a_m11, double a_m12, double a_m13,
                       double a_m21, double a_m22, double a_m23,
                       double a_m31, double a_m32, double a_m33)
        {
            M11 = a_m11;
            M12 = a_m12;
            M13 = a_m13;

            M21 = a_m21;
            M22 = a_m22;
            M23 = a_m23;

            M31 = a_m31;
            M32 = a_m32;
            M33 = a_m33;
        }

        public Matrix3(Matrix3 a_m)
        {
            M11 = a_m.M11;
            M12 = a_m.M12;
            M13 = a_m.M13;

            M21 = a_m.M21;
            M22 = a_m.M22;
            M23 = a_m.M23;

            M31 = a_m.M31;
            M32 = a_m.M32;
            M33 = a_m.M33;
        }

        public Matrix3(Vector2 a_col1, Vector2 a_col2)
        {
            M11 = a_col1.X;
            M12 = a_col2.X;
            M13 = 0;

            M21 = a_col1.Y;
            M22 = a_col2.Y;
            M23 = 0;

            M31 = 0;
            M32 = 0;
            M33 = 1;
        }

        public Matrix3(Vector2 a_col1, Vector2 a_col2, Vector2 a_col3)
        {
            M11 = a_col1.X;
            M12 = a_col2.X;
            M13 = a_col3.X;

            M21 = a_col1.Y;
            M22 = a_col2.Y;
            M23 = a_col3.Y;

            M31 = 0;
            M32 = 0;
            M33 = 1;
        }

        public static Matrix3 operator +(Matrix3 a_a, Matrix3 a_b)
        {
            return new Matrix3(

                a_a.M11 + a_b.M11,
                a_a.M12 + a_b.M12,
                a_a.M13 + a_b.M13,

                a_a.M21 + a_b.M21,
                a_a.M22 + a_b.M22,
                a_a.M23 + a_b.M23,

                a_a.M31 + a_b.M31,
                a_a.M32 + a_b.M32,
                a_a.M33 + a_b.M33);
        }

        public static Matrix3 operator -(Matrix3 a_a, Matrix3 a_b)
        {
            return new Matrix3(

                a_a.M11 - a_b.M11,
                a_a.M12 - a_b.M12,
                a_a.M13 - a_b.M13,

                a_a.M21 - a_b.M21,
                a_a.M22 - a_b.M22,
                a_a.M23 - a_b.M23,

                a_a.M31 - a_b.M31,
                a_a.M32 - a_b.M32,
                a_a.M33 - a_b.M33);
        }

        public Matrix3 Transposed
        {
            get
            {
                return new Matrix3(

                    M11, M21, M31,
                    M12, M22, M32,
                    M13, M23, M33);
            }
        }

        public static Matrix3 operator *(Matrix3 a_a, Matrix3 a_b)
        {
            return new Matrix3(

                a_a.M11 * a_b.M11 + a_a.M12 * a_b.M21 + a_a.M13 * a_b.M31,
                a_a.M11 * a_b.M12 + a_a.M12 * a_b.M22 + a_a.M13 * a_b.M32,
                a_a.M11 * a_b.M13 + a_a.M12 * a_b.M23 + a_a.M13 * a_b.M33,

                a_a.M21 * a_b.M11 + a_a.M22 * a_b.M21 + a_a.M23 * a_b.M31,
                a_a.M21 * a_b.M12 + a_a.M22 * a_b.M22 + a_a.M23 * a_b.M32,
                a_a.M21 * a_b.M13 + a_a.M22 * a_b.M23 + a_a.M23 * a_b.M33,

                a_a.M31 * a_b.M11 + a_a.M32 * a_b.M21 + a_a.M33 * a_b.M31,
                a_a.M31 * a_b.M12 + a_a.M32 * a_b.M22 + a_a.M33 * a_b.M32,
                a_a.M31 * a_b.M13 + a_a.M32 * a_b.M23 + a_a.M33 * a_b.M33);
        }

        public static Vector3 operator *(Matrix3 a_a, Vector3 a_b)
        {
            return new Vector3(

                (a_a.M11 * a_b.X) + (a_a.M12 * a_b.Y) + (a_a.M13 * a_b.Z),
                (a_a.M21 * a_b.X) + (a_a.M22 * a_b.Y) + (a_a.M23 * a_b.Z),
                (a_a.M31 * a_b.X) + (a_a.M32 * a_b.Y) + (a_a.M33 * a_b.Z));
        }

        public static Vector2 operator *(Matrix3 a_a, Vector2 a_b)
        {
            double x = (a_a.M11 * a_b.X) + (a_a.M12 * a_b.Y) + a_a.M13;
            double y = (a_a.M21 * a_b.X) + (a_a.M22 * a_b.Y) + a_a.M23;
            double w = (a_a.M31 * a_b.X) + (a_a.M32 * a_b.Y) + a_a.M33;

            Debug.Assert(w != 0);

            return new Vector2(x / w, y / w);
        }

        public override int GetHashCode()
        {
            return M11.GetHashCode() ^
                   M12.GetHashCode() ^
                   M13.GetHashCode() ^

                   M21.GetHashCode() ^
                   M22.GetHashCode() ^
                   M23.GetHashCode() ^

                   M31.GetHashCode() ^
                   M32.GetHashCode() ^
                   M33.GetHashCode();
        }

        public override bool Equals(object a_obj)
        {
            if (a_obj == null)
                return false;

            if (!typeof(Matrix3).Equals(a_obj.GetType()))
                return false;
            Matrix3 matrix3d3 = (Matrix3)a_obj;

            return (M11 == matrix3d3.M11) &&
                   (M12 == matrix3d3.M12) &&
                   (M13 == matrix3d3.M13) &&

                   (M21 == matrix3d3.M21) &&
                   (M22 == matrix3d3.M22) &&
                   (M23 == matrix3d3.M23) &&

                   (M31 == matrix3d3.M31) &&
                   (M32 == matrix3d3.M32) &&
                   (M33 == matrix3d3.M33);
        }

        public bool Equals(Matrix3 a_matrix3d3)
        {
            return (M11 == a_matrix3d3.M11) &&
                   (M12 == a_matrix3d3.M12) &&
                   (M13 == a_matrix3d3.M13) &&

                   (M21 == a_matrix3d3.M21) &&
                   (M22 == a_matrix3d3.M22) &&
                   (M23 == a_matrix3d3.M23) &&

                   (M31 == a_matrix3d3.M31) &&
                   (M32 == a_matrix3d3.M32) &&
                   (M33 == a_matrix3d3.M33);
        }

        public override string ToString()
        {
            const int SIZE = 3;

            string r = "";

            for (int y = 0; y < SIZE; y++)
            {
                r = r + "( ";

                for (int x = 0; x < SIZE; x++)
                {
                    r = r + this[y * SIZE + x].ToString(CultureInfo.InvariantCulture);

                    if (x != SIZE - 1)
                        r = r + ", ";
                }

                r = r + " ) ";
            }

            return r;
        }

        public bool IsAlmostRelativeEquals(Matrix3 a_m, double a_precision = Constants.DOUBLE_PRECISION)
        {
            for (int i = 0; i < 9; i++)
            {
                if (!this[i].IsAlmostRelativeEquals(a_m[i], a_precision))
                    return false;
            }

            return true;
        }

        public static bool operator ==(Matrix3 a_a, Matrix3 a_b)
        {
            return (a_a.M11 == a_b.M11) &&
                   (a_a.M12 == a_b.M12) &&
                   (a_a.M13 == a_b.M13) &&

                   (a_a.M21 == a_b.M21) &&
                   (a_a.M22 == a_b.M22) &&
                   (a_a.M23 == a_b.M23) &&

                   (a_a.M31 == a_b.M31) &&
                   (a_a.M32 == a_b.M32) &&
                   (a_a.M33 == a_b.M33);
        }

        public static bool operator !=(Matrix3 a_a, Matrix3 a_b)
        {
            return !(a_a == a_b);
        }

        public Vector2 Col1
        {
            get
            {
                return new Vector2(M11, M21);
            }
        }

        public Matrix3 SetCol1(Vector2 a_v)
        {
            return new Matrix3(

                a_v.X,
                M12,
                M13,

                a_v.Y,
                M22,
                M23,

                M31,
                M32,
                M33);
        }

        public Vector2 Col2
        {
            get
            {
                return new Vector2(M12, M22);
            }
        }

        public Matrix3 SetCol2(Vector2 a_v)
        {
            return new Matrix3(

                M11,
                a_v.X,
                M13,

                M21,
                a_v.Y,
                M23,

                M31,
                M32,
                M33);
        }

        public Vector2 Col3
        {
            get
            {
                return new Vector2(M13, M23);
            }
        }

        public Matrix3 SetCol3(Vector2 a_v)
        {
            return new Matrix3(

                M11,
                M12,
                a_v.X,

                M21,
                M22,
                a_v.Y,

                M31,
                M32,
                M33);
        }

        public double Determinant
        {
            get
            {
                return M11 * M22 * M33 +
                        M12 * M23 * M31 +
                        M13 * M21 * M32 -
                        M11 * M23 * M32 -
                        M12 * M21 * M33 -
                        M13 * M22 * M31;
            }
        }

        public Matrix3 Inverted
        {
            get
            {
                double det = Determinant;

                Debug.Assert(det != 0);

                return new Matrix3(

                    (M22 * M33 - M23 * M32) / det,
                    -(M12 * M33 - M13 * M32) / det,
                    (M12 * M23 - M13 * M22) / det,

                    -(M21 * M33 - M23 * M31) / det,
                    (M11 * M33 - M13 * M31) / det,
                    -(M11 * M23 - M13 * M21) / det,

                    (M21 * M32 - M22 * M31) / det,
                    -(M11 * M32 - M12 * M31) / det,
                    (M11 * M22 - M12 * M21) / det
                );
            }
        }

        public double[] ToArray()
        {
            return new double[]
            {
                M11, M12, M13,
                M21, M22, M23,
                M31, M32, M33
            };
        }

        public double this[int a_index]
        {
            get
            {
                switch (a_index)
                {
                    case 0: return M11;
                    case 1: return M12;
                    case 2: return M13;

                    case 3: return M21;
                    case 4: return M22;
                    case 5: return M23;

                    case 6: return M31;
                    case 7: return M32;
                    case 8: return M33;
                }

                throw new IndexOutOfRangeException();
            }
        }

        public double this[int a_x, int a_y]
        {
            get
            {
                Debug.Assert(a_x >= 0);
                Debug.Assert(a_y >= 0);
                Debug.Assert(a_x < 3);
                Debug.Assert(a_y < 3);

                return this[a_y * 3 + a_x];
            }
        }

        [Conditional("DEBUG")]
        public void CheckAxisAlligned()
        {
            Debug.Assert(Col1.Length.IsAlmostRelativeEquals(1));
            Debug.Assert(Col2.Length.IsAlmostRelativeEquals(1));
            Debug.Assert((Col1 * Col2) == 0);
        }

        /// <summary>
        /// Angle in deg.
        /// </summary>
        /// <param name="a_angle"></param>
        /// <returns></returns>
        public static Matrix3 CreateRotate(double a_angle)
        {
            double sin = Math.Sin(MathExtensions.ToRad(a_angle));
            double cos = Math.Cos(MathExtensions.ToRad(a_angle));

            return new Matrix3(

                cos, sin, 0,
               -sin, cos, 0,
                  0, 0, 1);
        }

        public static Matrix3 CreateTranslation(Vector2 a_v)
        {
            return CreateTranslation(a_v.X, a_v.Y);
        }

        public static Matrix3 CreateTranslation(double a_x, double a_y)
        {
            return new Matrix3(

                  1, 0, a_x,
                  0, 1, a_y,
                  0, 0, 1);
        }
    }
}